package clubhouse.kalmanfilter;

import java.util.ListIterator;
import java.util.Vector;

import Jama.Matrix;

import com.mathworks.toolbox.javabuilder.MWClassID;
import com.mathworks.toolbox.javabuilder.MWException;
import com.mathworks.toolbox.javabuilder.MWNumericArray;



//Be careful with following imports, depend of the structure of your ekf.jar.
import ekf.filter;
import ekf.functions;
import ekf.util;
/*import clubhouse.ekf.filter;
import clubhouse.ekf.functions;
import clubhouse.ekf.util;*/

/**
 * @author Anis Darwich & Lionel Croix - Simulation and experimentation on Robot in WSN - WnSN Lab - SJTU
 */
public class Ekf {
	
	static {
		//Uncomment the next line to display your java.library.path.
		//System.out.println(System.getProperty("java.library.path"));
		//Add all dependent dll of nativemc176 library to make the EKF work.
		System.loadLibrary("MCLMCRRT76");
	}
	
	/** Used to store the coordinates of the landmarks**/
	private double[] xS;
	private double[] yS;
	
	/** Used to discretize the time-continuous model**/
	private double dt = 0.1;
	
	/** Used to generate the measurement noise covariance matrix**/
	private double sd = 0.05;
	
	private double[][] m;
	private double[][] p = new double[][] {
			{1, 0, 0, 0},
			{0, 1, 0, 0},
			{0, 0, 1, 0},
			{0, 0, 0, 1}};
	
	private functions c;
	private filter f;
	private util u;
	
	/** Discretized dynamic model matrix **/
	private MWNumericArray A;
	/** Discrete process covariance matrix **/
	private MWNumericArray Q;
	
	/** State mean estimate vector **/
	private MWNumericArray M;
	/** State mean covariance matrix **/
	private MWNumericArray P = new MWNumericArray(p, MWClassID.DOUBLE);
	/** Measurement noise covariance matrix **/
	private MWNumericArray R;
	/** Default: identity **/
	private MWNumericArray V;
	
	/** State vector **/
	private MWNumericArray X;
	/** Measurement vector **/
	private MWNumericArray Y;
	/** Landmarks coordinates matrix **/
	private MWNumericArray S;
	
	private ListIterator<double[]> li;
	
	/**
	 * Constructs the Ekf object. Instantiate objects functions, filter and util.
	 */
	public Ekf() {
		try {
			// Initialize the classes from the library
			c = new functions();
			f = new filter();
			u = new util();
		} catch (MWException e) {
			e.printStackTrace();
		}
		this.X = new MWNumericArray(new double[][] {{0}, {0}, {0}, {0}}, MWClassID.DOUBLE);
	}
	
	/**
	 * Generates the discretized dynamic model of the system and the discrete process covariance.
	 * Calculate the transition matrix A and the covariance of the discrete process matrix Q.
	 */
	public void initCalcul() {
		double[][] f = new double[][]{
				{0, 0, 1, 0},
				{0, 0, 0, 1},
				{0, 0, 0, 0},
				{0, 0, 0, 0}};
		// Default values
		double[][] l = new double[][]{
				{1, 0, 0, 0},
				{0, 1, 0, 0},
				{0, 0, 1, 0},
				{0, 0, 0, 1}};
		// Default values
		double[][] qc = new double[][]{
				{0, 0, 0, 0},
				{0, 0, 0, 0},
				{0, 0, 0.1, 0},
				{0, 0, 0, 0.1}};
		
		MWNumericArray F = new MWNumericArray(f, MWClassID.DOUBLE);
		MWNumericArray L = new MWNumericArray(l, MWClassID.DOUBLE);
		MWNumericArray Qc = new MWNumericArray(qc, MWClassID.DOUBLE);
		
		// List of output objects
		Object[] rhs = new Object[] {this.A, this.Q};
		// List of input objects
		Object[] lhs = new Object[] {F, L, Qc, this.dt};
		
		try {
			u.lti_disc(rhs, lhs);
			//Store output data in corresponding object
			this.A = (MWNumericArray) rhs[0];
			this.Q = (MWNumericArray) rhs[1];
		} catch (MWException e) {
			e.printStackTrace();
		}
		
		//delete data structure instance
		F.dispose();
		L.dispose();
		Qc.dispose();
	}
	
	/**
	 * Initialise the vector containing the state mean estimate using the initial position.
	 * @param x X coordinate.
	 * @param y Y coordinate.
	 * @param dx Speed along the X axis.
	 * @param dy Speed along the Y axis.
	 */
	public void setInitialPosition(double x, double y, double dx, double dy){
		this.m = new double[][] {{x},{y},{dx},{dy}};
		this.M = new MWNumericArray(m, MWClassID.DOUBLE);
	}
	
	/**
	 * Initialise the measurement processing by arranging the observed landmarks coordinates in a matrix.
	 * @param nodes All the observed landmarks.
	 */
	public void initMeasurement(Vector<double[]> nodes){
		double[] tempLandmark;
		int i = 0;
		
		xS = new double[nodes.size()];
		yS = new double[nodes.size()];
		
		setRandV(nodes.size());
		
		li = nodes.listIterator();
		while(li.hasNext()){
        	tempLandmark = li.next();
        	xS[i] = tempLandmark[1];
        	yS[i] = tempLandmark[2];
        	i++;
		}
		
		this.S = new MWNumericArray(new double[][] {xS, yS}, MWClassID.DOUBLE);
		measurement();
	}
	
	/**
	 * Calculate the measurement matrix.
	 */
	public void measurement() {
		// List of output objects
		Object[] rhs = new Object[] {this.Y};
		// List of input objects
		Object[] lhs = new Object[] {this.X, this.S};
		
		try {
			c.bot_h(rhs, lhs);
			//Store output data in corresponding object
			this.Y = (MWNumericArray) rhs[0];
		} catch (MWException e) {
			e.printStackTrace();
		}
		
	}
	
	/**
	 * Return the state mean estimate after the prediction.
	 * @return State mean estimate vector.
	 */
	public double[][] getMEstimate() {
		return (double[][]) this.M.toDoubleArray();
	}
	
	/**
	 * Return the state covariance after the prediction.
	 * @return State covariance vector.
	 */
	public double[][] getPEstimate() {
		return (double[][]) this.P.toDoubleArray();
	}
	
	/**
	 * Main filter function. Includes the predict and update functions.
	 */
	public void calcul() {
		//List of output objects
		Object[] rhs = new Object[] {this.M, this.P};
		// List of input objects
		Object[] lhs = new Object[] {this.M, this.P, this.A, this.Q};
		try {
			f.ekf_predict1(rhs, lhs);
			//Store output data in corresponding object
			this.M = (MWNumericArray) rhs[0];
			this.P = (MWNumericArray) rhs[1];
		} catch (MWException e) {
			e.printStackTrace();
		}
		
		String dh = "bot_dh_dx";
		String h = "bot_h";
		
		// List of input objects
		Object[] lhs2 = new Object[] {this.M, this.P, this.Y, dh/*this.tmp_dY*/, this.R, h/*this.tmp_Y*/, this.V, this.S};
		try {
			f.ekf_update1(rhs, lhs2);
			//Store output data in corresponding object
			this.M = (MWNumericArray) rhs[0];
			this.P = (MWNumericArray) rhs[1];
		} catch (MWException e) {
			e.printStackTrace();
		}
		
		//delete data structure instance
		this.S.dispose();
		this.R.dispose();
		this.V.dispose();
	}
	
	/**
	 * Set the new coordinates of the robot after motion.
	 * @param x New X coordinate.
	 * @param y New Y coordinate.
	 * @param dx Default: 0.
	 * @param dy Default: 0.
	 */
	public void setMotion(double x, double y, double dx, double dy) {
		int[] index = new int[] {0, 1};
		index[0] = 1;
		this.X.set(index, x);
		index[0] = 2;
		this.X.set(index, y);
		index[0] = 3;
		this.X.set(index, dx);
		index[0] = 4;
		this.X.set(index, dy);
	}
	
	/**
	 * Set the measurement noise covariance matrix R and the derivative of measurement function with respect
	 * to the noise matrix V (default: identity).
	 * @param size Number of observed landmarks.
	 */
	public void setRandV(int size) {
		this.R = new MWNumericArray((Matrix.identity(size, size).timesEquals(sd*sd)).getArray(), MWClassID.DOUBLE);
		this.V = new MWNumericArray((Matrix.identity(size, size)).getArray(), MWClassID.DOUBLE);
	}
	
}
